function Jacobian = calculate_Jacobian(DH_para, q_delta, q)

px = q_delta(1); py = q_delta(2); pz = q_delta(3);
rz = q_delta(4); ry = q_delta(5); rx = q_delta(6);
tx = q_delta(7); ty = q_delta(8); tz = q_delta(9);

T_r = fkine(DH_para, q);
nx_r = T_r(1, 1); ny_r = T_r(2, 1); nz_r = T_r(3, 1); 
ox_r = T_r(1, 2); oy_r = T_r(2, 2); oz_r = T_r(3, 2); 
ax_r = T_r(1, 3); ay_r = T_r(2, 3); az_r = T_r(3, 3); 
px_r = T_r(1, 4); py_r = T_r(2, 4); pz_r = T_r(3, 4); 

Jacobian = [ 1, 0, 0, pz_r*cos(rz)*sin(rx) - px_r*cos(ry)*sin(rz) - py_r*cos(rx)*cos(rz) - pz_r*cos(rx)*sin(ry)*sin(rz) - py_r*sin(rx)*sin(ry)*sin(rz) - ay_r*tz*cos(rx)*cos(rz) - ny_r*tx*cos(rx)*cos(rz) - oy_r*ty*cos(rx)*cos(rz) - ax_r*tz*cos(ry)*sin(rz) + az_r*tz*cos(rz)*sin(rx) - nx_r*tx*cos(ry)*sin(rz) + nz_r*tx*cos(rz)*sin(rx) - ox_r*ty*cos(ry)*sin(rz) + oz_r*ty*cos(rz)*sin(rx) - az_r*tz*cos(rx)*sin(ry)*sin(rz) - nz_r*tx*cos(rx)*sin(ry)*sin(rz) - oz_r*ty*cos(rx)*sin(ry)*sin(rz) - ay_r*tz*sin(rx)*sin(ry)*sin(rz) - ny_r*tx*sin(rx)*sin(ry)*sin(rz) - oy_r*ty*sin(rx)*sin(ry)*sin(rz), cos(rz)*(pz_r*cos(rx)*cos(ry) - ax_r*tz*sin(ry) - nx_r*tx*sin(ry) - ox_r*ty*sin(ry) - px_r*sin(ry) + py_r*cos(ry)*sin(rx) + az_r*tz*cos(rx)*cos(ry) + nz_r*tx*cos(rx)*cos(ry) + oz_r*ty*cos(rx)*cos(ry) + ay_r*tz*cos(ry)*sin(rx) + ny_r*tx*cos(ry)*sin(rx) + oy_r*ty*cos(ry)*sin(rx)),   py_r*(sin(rx)*sin(rz) + cos(rx)*cos(rz)*sin(ry)) + pz_r*(cos(rx)*sin(rz) - cos(rz)*sin(rx)*sin(ry)) + tz*(ay_r*(sin(rx)*sin(rz) + cos(rx)*cos(rz)*sin(ry)) + az_r*(cos(rx)*sin(rz) - cos(rz)*sin(rx)*sin(ry))) + tx*(ny_r*(sin(rx)*sin(rz) + cos(rx)*cos(rz)*sin(ry)) + nz_r*(cos(rx)*sin(rz) - cos(rz)*sin(rx)*sin(ry))) + ty*(oy_r*(sin(rx)*sin(rz) + cos(rx)*cos(rz)*sin(ry)) + oz_r*(cos(rx)*sin(rz) - cos(rz)*sin(rx)*sin(ry))), nz_r*(sin(rx)*sin(rz) + cos(rx)*cos(rz)*sin(ry)) - ny_r*(cos(rx)*sin(rz) - cos(rz)*sin(rx)*sin(ry)) + nx_r*cos(ry)*cos(rz), oz_r*(sin(rx)*sin(rz) + cos(rx)*cos(rz)*sin(ry)) - oy_r*(cos(rx)*sin(rz) - cos(rz)*sin(rx)*sin(ry)) + ox_r*cos(ry)*cos(rz), az_r*(sin(rx)*sin(rz) + cos(rx)*cos(rz)*sin(ry)) - ay_r*(cos(rx)*sin(rz) - cos(rz)*sin(rx)*sin(ry)) + ax_r*cos(ry)*cos(rz);
 0, 1, 0, px_r*cos(ry)*cos(rz) - py_r*cos(rx)*sin(rz) + pz_r*sin(rx)*sin(rz) + az_r*tz*sin(rx)*sin(rz) + nz_r*tx*sin(rx)*sin(rz) + oz_r*ty*sin(rx)*sin(rz) + pz_r*cos(rx)*cos(rz)*sin(ry) + py_r*cos(rz)*sin(rx)*sin(ry) + ax_r*tz*cos(ry)*cos(rz) + nx_r*tx*cos(ry)*cos(rz) + ox_r*ty*cos(ry)*cos(rz) - ay_r*tz*cos(rx)*sin(rz) - ny_r*tx*cos(rx)*sin(rz) - oy_r*ty*cos(rx)*sin(rz) + az_r*tz*cos(rx)*cos(rz)*sin(ry) + nz_r*tx*cos(rx)*cos(rz)*sin(ry) + oz_r*ty*cos(rx)*cos(rz)*sin(ry) + ay_r*tz*cos(rz)*sin(rx)*sin(ry) + ny_r*tx*cos(rz)*sin(rx)*sin(ry) + oy_r*ty*cos(rz)*sin(rx)*sin(ry), sin(rz)*(pz_r*cos(rx)*cos(ry) - ax_r*tz*sin(ry) - nx_r*tx*sin(ry) - ox_r*ty*sin(ry) - px_r*sin(ry) + py_r*cos(ry)*sin(rx) + az_r*tz*cos(rx)*cos(ry) + nz_r*tx*cos(rx)*cos(ry) + oz_r*ty*cos(rx)*cos(ry) + ay_r*tz*cos(ry)*sin(rx) + ny_r*tx*cos(ry)*sin(rx) + oy_r*ty*cos(ry)*sin(rx)), - py_r*(cos(rz)*sin(rx) - cos(rx)*sin(ry)*sin(rz)) - pz_r*(cos(rx)*cos(rz) + sin(rx)*sin(ry)*sin(rz)) - tz*(ay_r*(cos(rz)*sin(rx) - cos(rx)*sin(ry)*sin(rz)) + az_r*(cos(rx)*cos(rz) + sin(rx)*sin(ry)*sin(rz))) - tx*(ny_r*(cos(rz)*sin(rx) - cos(rx)*sin(ry)*sin(rz)) + nz_r*(cos(rx)*cos(rz) + sin(rx)*sin(ry)*sin(rz))) - ty*(oy_r*(cos(rz)*sin(rx) - cos(rx)*sin(ry)*sin(rz)) + oz_r*(cos(rx)*cos(rz) + sin(rx)*sin(ry)*sin(rz))), ny_r*(cos(rx)*cos(rz) + sin(rx)*sin(ry)*sin(rz)) - nz_r*(cos(rz)*sin(rx) - cos(rx)*sin(ry)*sin(rz)) + nx_r*cos(ry)*sin(rz), oy_r*(cos(rx)*cos(rz) + sin(rx)*sin(ry)*sin(rz)) - oz_r*(cos(rz)*sin(rx) - cos(rx)*sin(ry)*sin(rz)) + ox_r*cos(ry)*sin(rz), ay_r*(cos(rx)*cos(rz) + sin(rx)*sin(ry)*sin(rz)) - az_r*(cos(rz)*sin(rx) - cos(rx)*sin(ry)*sin(rz)) + ax_r*cos(ry)*sin(rz);
 0, 0, 1,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      0,                     - px_r*cos(ry) - tz*(ax_r*cos(ry) + az_r*cos(rx)*sin(ry) + ay_r*sin(rx)*sin(ry)) - tx*(nx_r*cos(ry) + nz_r*cos(rx)*sin(ry) + ny_r*sin(rx)*sin(ry)) - ty*(ox_r*cos(ry) + oz_r*cos(rx)*sin(ry) + oy_r*sin(rx)*sin(ry)) - pz_r*cos(rx)*sin(ry) - py_r*sin(rx)*sin(ry),                                                                                                                                                                                                                                                                                      cos(ry)*(py_r*cos(rx) - pz_r*sin(rx) + ay_r*tz*cos(rx) + ny_r*tx*cos(rx) + oy_r*ty*cos(rx) - az_r*tz*sin(rx) - nz_r*tx*sin(rx) - oz_r*ty*sin(rx)),                                                                 nz_r*cos(rx)*cos(ry) - nx_r*sin(ry) + ny_r*cos(ry)*sin(rx),                                                                 oz_r*cos(rx)*cos(ry) - ox_r*sin(ry) + oy_r*cos(ry)*sin(rx),                                                                 az_r*cos(rx)*cos(ry) - ax_r*sin(ry) + ay_r*cos(ry)*sin(rx)];
